#include "gm6020.h"
#include "can_interface.h"
#include "common_math.h"
#include "user_global.h"

/**
 * @brief Update the motor speed
 *
 * @param uint8_t   type: speed feedback type
 *                  0 : encoder original speed feedback
 *                  1 : encoder speed feedback filtered by kalman filter
 *                  2 : external speed feedback
 * 
 * @param float     input: speed value (degree / s)  when "type" is "2"
 *                         plus or minus  when "type" is "0" or "1"
 *               
 */
void GM6020::SpeedUpdate(uint8_t type, float input)
{
    if (type == 0) {
        m_speed_current = input * m_speed_current_encoder;
    } else if (type == 1) {
        m_speed_current = input * m_kalman_filter_speed->GetFilterOutput();
    } else if (type == 2) {
        m_speed_current_external = input;
        m_speed_current = m_speed_current_external;
    }
}



/**
 * @brief Update the motor angle
 *
 * @param uint8_t   type: angle feedback type
 *                  0 : encoder original angle feedback
 *                  1 : encoder angle feedback filtered by kalman filter
 *                  2 : external angle feedback
 *                  3 : LPF
 * 
 * @param float     input: angle value (degree)  when "type" is "2"
 *                         plus or minus  when "type" is "0" or "1"
 *                          
 */
void GM6020::AngleUpdate(uint8_t type, float input)
{
    if (type == 0) {
        m_angle_current = input * m_angle_current_encoder;
    } else if (type == 1) {
        m_angle_current = input * m_kalman_filter_angle->GetFilterOutput();
    } else if (type == 2) {
        m_angle_current_external = input;
        m_angle_current = m_angle_current_external;
    }
}



/**
 * @brief Update the motor speed measured by encoder
 *
 * @param float speed: the input speed of the motor reducer (degree / s)
 */
void GM6020::EncoderSpeedUpdate(float speed)
{
    m_speed_current_encoder = speed / m_reduction_ratio / 60 * 360;
    m_kalman_filter_speed->UpdateFilter(m_speed_current_encoder);
    m_speed_current_encoder_filter = m_kalman_filter_speed->GetFilterOutput();
}



/**
 * @brief Update the motor angle measured by encoder
 *
 * @param uint32_t value: the feedback encoder value
 */
void GM6020::EncoderAngleUpdate(uint32_t value)
{
    m_encoder->EncodeValueUpdate(value);
    m_angle_current_encoder = m_encoder->m_sum_value / 
    m_encoder->m_resolution * 360.0f / m_reduction_ratio;
    m_kalman_filter_angle->UpdateFilter(m_angle_current_encoder);
    m_angle_current_encoder_filter = m_kalman_filter_angle->GetFilterOutput();
}



/**
 * @brief Update the motor angle
 *
 * @param 
 */
void GM6020::SpeedControl(void)
{
    m_speed_pid->CalWeakenPID(m_speed_target - m_speed_current);
}


//TODO：【云台】云台电机（6020）TD+PID控制 ，电机PID计算都在电机文件里
/**
 * @brief Update the motor angle
 *
 * @param 
 */
void GM6020::AngleControl(void)
{
    m_angle_td->CalAdrcTD(m_angle_target);
    m_angle_pid->CalWeakenPID(m_angle_td->m_x1 - m_angle_current);
    m_speed_target = m_angle_pid->m_output + m_angle_td->m_k * m_angle_td->m_x2;
    m_speed_pid->CalWeakenPID(m_speed_target - m_speed_current);
}



/**
 * @brief 重力前馈
 *
 * @param 
 * 
 * @retval int: 重力前馈值
 * 
 * @note //TODO:【待优化】重力前馈计算.线性插值？据悉，前人已经尝试过余弦了，但是效果不太好。缓慢移动PITCH，保存重力前馈，尽量求出解析解吧。pp哥的线性前馈挺好使的~
 */
int GM6020::GetGravityFeedForward(float angle_current)
{
    if(angle_current < ff_table.start_angle)
        return ff_table.gra_table_data[0];
    else if(angle_current > ff_table.end_angle)
        return ff_table.gra_table_data[ff_table.table_length-1];
    else
    {
        int inter1_resolve;
        uint8_t pos;
        float prop;//待优化
        pos = (uint8_t)(((angle_current-ff_table.start_angle)/(ff_table.end_angle - ff_table.start_angle)) /
        (1.0/ (float)(ff_table.table_length-1)));
        prop = ((angle_current-ff_table.start_angle)/(ff_table.end_angle - ff_table.start_angle) - (1.0/ (float)(ff_table.table_length-1))*pos) /
        (1.0/ (float)(ff_table.table_length-1));
        inter1_resolve = ff_table.gra_table_data[pos] * (1-prop) + ff_table.gra_table_data[pos+1] * (prop);
        return inter1_resolve;
    }
}



/**
 * @brief 摩擦力前馈
 *
 * @param angle_current:当前角度   speed_current: 当前速度
 * 
 * @retval int: 摩擦力前馈值
 * 
 * @note //TODO:【待优化】摩擦力前馈计算.线性插值？
 */
int GM6020::GetFrictionFeedForward(float angle_current, float speed_current)
{
    int frcition_ff_current;

    if(angle_current < ff_table.start_angle)
        frcition_ff_current =  ff_table.fri_table_data[0];
    else if(angle_current > ff_table.end_angle)
        frcition_ff_current =  ff_table.fri_table_data[ff_table.table_length-1];
    else
    {
        int inter1_resolve;
        uint8_t pos;
        float prop;
        pos = (uint8_t)(((angle_current-ff_table.start_angle)/(ff_table.end_angle - ff_table.start_angle)) /
        (1.0/ (float)(ff_table.table_length-1)));
        prop = ((angle_current-ff_table.start_angle)/(ff_table.end_angle - ff_table.start_angle) - (1.0/ (float)(ff_table.table_length-1))*pos) /
        (1.0/ (float)(ff_table.table_length-1));
        inter1_resolve = ff_table.fri_table_data[pos] * (1-prop) + ff_table.fri_table_data[pos+1] * (prop);
        frcition_ff_current = inter1_resolve;
    }
//速度判定，当速度很慢时，摩擦力前馈为0，速度为正时，摩擦力前馈为正，速度为负时，摩擦力前馈为负
    if(speed_current > ff_table.speed_dead_zone)
        frcition_ff_current = frcition_ff_current;
    else if(speed_current < -ff_table.speed_dead_zone)
        frcition_ff_current = -frcition_ff_current;
    else
        frcition_ff_current = 0;
    
    return frcition_ff_current;
}